#include "PDController.h"

PDController* PDController::_instance = 0;

PDController::PDController(void)
{
}


PDController::~PDController(void)
{
}

PDController* PDController::instance() {
	if (!_instance)
		_instance = new PDController();

	return _instance;
}

Command PDController::GetComand(tank_t* tank, Vector2 & trajectory) {
	float tthreshold = M_PI / 2.0f;
	double tmag = 40;
	double speed = 0;

	double dtheta = trajectory.angle() - normalizeAngle(tank->angle);

	if(dtheta > M_PI)
		dtheta -= 2.0f*M_PI;
	else if(dtheta < -M_PI)
		dtheta += 2.0f*M_PI;

	double angvel = 0;
	if (dtheta > tthreshold)
		angvel = 1;
	else if (abs(dtheta) > tthreshold)
		angvel = -1;
	else
		angvel = dtheta / tthreshold;

	Vector2 tank_cur_trajectory(cos(tank->angle), sin(tank->angle));

	double dot = Vector2::dot(trajectory, tank_cur_trajectory);

	//if (dist < 2 * tmag) {
	//	speed *= 1.0 / (2.0 - dot);
	//}

	speed = 1.0 / (1.0 + 2.0*fabs(2 * dtheta));

	return Command(tank->index, speed, angvel, false);
}

double PDController::normalizeAngle(double angle) {
	if (angle > 0) {
		while (angle > 2 * M_PI) {
			angle -= 2 * M_PI;
		}
	} else {
		while (angle < 0) {
			angle += 2 * M_PI;
		}
	}

	return angle;
}